/**
 * Copyright (c) 2009 blurredVision
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
 * associated documentation files (the "Software"), to deal in the Software without restriction, 
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or 
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT 
 * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT 
 * OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include "StdAfx.h"
#include "Quaternion.h"
#include <iostream>

#ifdef _WIN32
#include <windows.h>
#endif

#define NOOPENGL

#ifndef NOOPENGL
#include <GL/glew.h>
#include <GL/gl.h>
#endif

#include <math.h>
#include <assert.h>

namespace math {
#define PI 3.141592654f
#define HALFPI (PI/2.0f)
	
const Quaternion Quaternion::ZERO(0.0,0.0,0.0,0.0);
const Quaternion Quaternion::IDENTITY(0.0,0.0,0.0,1.0);
const float Quaternion::EPSILON = 1E-5f;

Quaternion::Quaternion()
{
	reset();
}

Quaternion::Quaternion( const Quaternion& copyFrom )
{
	set( copyFrom );
}

Quaternion::Quaternion( float x, float y, float z, float w )
{
	setValues(x,y,z,w);
}

Quaternion& Quaternion::reset()
{
	x = 0.0;
	y = 0.0;
	z = 0.0;
	w = 1.0;
	return *this;
}

Quaternion& Quaternion::set( const Quaternion& copyFrom )
{
	x = copyFrom.x;
	y = copyFrom.y;
	z = copyFrom.z;
	w = copyFrom.w;

	return *this;
}

Quaternion& Quaternion::set( float x, float y, float z )
{
	Quaternion xQ( 1.0, 0.0, 0.0, x );
	Quaternion yQ( 0.0, 1.0, 0.0, y );
	Quaternion zQ( 0.0, 0.0, 1.0, z );

	set( xQ );

	*this *= yQ;
	*this *= zQ;

	return *this;
}

Quaternion& Quaternion::set(float x, float y, float z, float angle)
{
	// Convert to radians.
	angle *= (float)( PI / 180.0 );

	// Normalize the axis.
	float factor = x*x + y*y + z*z;

//	assert( factor != 0 );

  float scaleBy( 1.0f / sqrt( factor ) );
	x = x * scaleBy;
	y = y * scaleBy;
	z = z * scaleBy;

  // Build a quaternion!
	this->w = cos( angle / 2.0f );

	float sinHalfAngle( sin( angle / 2.0f ) );

	this->x = x * sinHalfAngle;
	this->y = y * sinHalfAngle;
	this->z = z * sinHalfAngle;

  return *this;
}

void Quaternion::slerp(const Quaternion& p, const Quaternion& q, float t)
{
	static float l;
	static float om,sinom;
	static float sp,sq;
	static Quaternion qq;

	l = p.x*q.x + p.y*q.y + p.z*q.z + p.w*q.w;

	if ((1.0+l) > EPSILON) {
	if (fabs(l) > 1.0)
		l /= (float)fabs(l);

		om    = (float)acos(l);
		sinom = (float)sin(om);

		if (fabs(sinom) > EPSILON) {
			sp = (float)sin((1.0f-t)*om)/sinom;
			sq = (float)sin(t*om)/sinom;
		}
		else {
			sp = 1.0f - t;
			sq = t;
		}
	} else {
		sp = (float)sin((1.0-t) * HALFPI);
		sq = (float)sin(t*HALFPI);
	}

	this->x = sp*p.x + sq*q.x;
	this->y = sp*p.y + sq*q.y;
	this->z = sp*p.z + sq*q.z;
	this->w = sp*p.w + sq*q.w;
}

void Quaternion::setValues(float x, float y, float z, float w)
{
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Quaternion Quaternion::operator-() const
{
	return Quaternion(-x, -y, -z, -w);
}

Quaternion& Quaternion::operator=( const Quaternion &a )
{
	x = a.x;
	y = a.y;
	z = a.z;
	w = a.w;

	return *this;
}

Quaternion Quaternion::operator+(const Quaternion &a) const
{
	return Quaternion( x + a.x, y + a.y, z + a.z, w + a.w );
}

Quaternion& Quaternion::operator+=(const Quaternion &a)
{
	x += a.x;
	y += a.y;
	z += a.z;
	w += a.w;

	return *this;
}

Quaternion Quaternion::operator-(const Quaternion &a) const
{
	return Quaternion( x - a.x, y - a.y, z - a.z, w - a.w );
}

Quaternion& Quaternion::operator-=(const Quaternion &a)
{
	x -= a.x;
	y -= a.y;
	z -= a.z;
	w -= a.w;

	return *this;
}

Quaternion Quaternion::operator*(const Quaternion &a) const
{
	return Quaternion(
					w*a.x + x*a.w + y*a.z - z*a.y,
					w*a.y + y*a.w + z*a.x - x*a.z,
					w*a.z + z*a.w + x*a.y - y*a.x,
					w*a.w - x*a.x - y*a.y - z*a.z );
}

Quaternion Quaternion::operator*(float a) const
{
	return Quaternion( x*a, y*a, z*a, w*a );
}

Quaternion& Quaternion::operator*=(const Quaternion &a)
{
	*this = *this * a;
	return *this;
}

Quaternion& Quaternion::operator*=(float a)
{
	x *= a;
	y *= a;
	z *= a;
	w *= a;
	return *this;
}
/*
Vector3 Quaternion::operator*(const Vector3 &a) const
{
	float xxzz = x*x - z*z;
	float wwyy = w*w - y*y;

	float xw2 = x*w*2.0f;
	float xy2 = x*y*2.0f;
	float xz2 = x*z*2.0f;
	float yw2 = y*w*2.0f;
	float yz2 = y*z*2.0f;
	float zw2 = z*w*2.0f;

	return Vector3(
		(xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
		(xy2 - zw2)*a.x			+ (xxzz - wwyy)*a.y		+ (yz2 + xw2)*a.z,
		(xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
	);
}
*/

bool Quaternion::compare(const Quaternion &a) const
{
	return ( (x == a.x)
				&& (y == a.y)
				&& (z == a.z)
				&& (w == a.w) );
}

bool Quaternion::operator==(const Quaternion &a) const
{
	return compare(a);
}

bool Quaternion::operator!=(const Quaternion &a) const
{
	return !compare(a);
}

float Quaternion::length() const
{
	return sqrt(x*x + y*y + z*z + w*w);
}

Quaternion Quaternion::conj()
{
	Quaternion q;
	q.x = -this->x;
	q.y = -this->y;
	q.z = -this->z;
	q.w = this->w;
	return q;
}

Quaternion& Quaternion::normalize()
{
	float len;
	float ilength;

	len = this->length();
	if ( len ) {
		ilength = 1 / len;
		x *= ilength;
		y *= ilength;
		z *= ilength;
		w *= ilength;
	}
	return *this;
}

math::Vector3 Quaternion::rotate(const math::Vector3 &vec)
{
	Quaternion t;
	Quaternion q = *this;
	t = q*vec*q.conj();
	return t.getVector();
}

math::Vector3 Quaternion::getVector() const
{
	return math::Vector3(x, y, z);
}

math::Matrix4 Quaternion::getMatrix4() const
{
	math::Matrix4	mat;
	float	wx, wy, wz;
	float	xx, yy, yz;
	float	xy, xz, zz;

	xx = x * x;
	xy = x * y;
	xz = x * z;

	yy = y * y;
	yz = y * z;
	zz = z * z;

	wx = w * x;
	wy = w * y;
	wz = w * z;

	#define M( y, x )  mat[x + y * 4]

	M( 0 , 0 ) = 1.0f - 2*( yy + zz );
	M( 0 , 1 ) = 2.0f * (xy - wz);
	M( 0 , 2 ) = 2.0f * (xz + wy);
	M( 0 , 3 ) = 0.0f;

	M( 1 , 0 ) = 2.0f * (xy + wz);
	M( 1 , 1 ) = 1.0f - 2.0f * ( xx + zz );
	M( 1 , 2 ) = 2.0f * (yz - wx);
	M( 1 , 3 ) = 0.0f;

	M( 2 , 0 ) = 2.0f * (xz - wy);
	M( 2 , 1 ) = 2.0f * (yz + wx);
	M( 2 , 2 ) = 1.0f - 2.0f*( xx + yy );
	M( 2 , 3 ) = 0.0f;

	M( 3 , 0 ) = 0.0f;
	M( 3 , 1 ) = 0.0f;
	M( 3 , 2 ) = 0.0f;
	M( 3 , 3 ) = 1.0f;

	return mat;
}

void Quaternion::print() const
{
	std::cout << "quat: " << x << " " << y << " " << z << " " << w << std::endl;
}

} // namespace
